//三方测试+视觉代码
#include <rclcpp/rclcpp.hpp>
#include <my_custom_msgs/msg/time_info.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>  // 添加力数据的消息类型
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/int32.hpp> // 添加 Int32 消息头文件
#include <QApplication>
#include <QWidget>
#include <QLabel>
#include <QPushButton>
#include <QTimer>
#include <QPixmap>
#include <QMovie>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QMessageBox>
#include <QProcess>
#include <QDebug>
#include "cartesian_path.h"
#include "cartesian_path_visual.h"
#include <thread>
#include <signal.h> // 添加 signal 头文件
#include <unistd.h> // 添加 unistd 头文件
#include <atomic> // 添加 atomic 头文件

bool force_sign = true;
bool Path1toVisual(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);
bool executeCartesianPath1(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);
bool executeCartesianPath2(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);
bool executeCartesianPath3(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);
bool returnToMiddle(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);

class ControlPanel : public QWidget {
    Q_OBJECT

public:
    ControlPanel(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr) : QWidget(parent), node_(node), move_group(node, "zc4"), last_result_value_(0) { // 初始化 last_result_value_
        setWindowTitle("助餐机器人控制面板");
        setFixedSize(500, 400);
        process = new QProcess(this);

        // 设置主布局
        QVBoxLayout* mainLayout = new QVBoxLayout(this);

        // 上方时间显示
        QHBoxLayout* topLayout = new QHBoxLayout();
        timeLabel = new QLabel("当前时间: 未接收到\n运行时间: 未接收到", this);
        topLayout->addWidget(timeLabel);
        mainLayout->addLayout(topLayout);

        QHBoxLayout* statusLayout = new QHBoxLayout();
        QVBoxLayout* INFOLayout = new QVBoxLayout();
        // 当前状态标签
        statusLabel = new QLabel("当前状态: 空闲", this);
        statusLabel->setStyleSheet("border: 1px solid black; font-size: 22px; padding: 10px;");
        statusLabel->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
        statusLabel->setMaximumWidth(220);
        INFOLayout->addWidget(statusLabel);

        // 取食路径标签
        pathLabel = new QLabel("取食路径: 无", this);
        pathLabel->setStyleSheet("border: 1px solid black; font-size: 22px; padding: 10px;");
        pathLabel->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
        pathLabel->setMaximumWidth(220);
        INFOLayout->addWidget(pathLabel);

        // 喂食速度标签
        pathSetLabel = new QLabel("喂食速度: 中", this);
        pathSetLabel->setStyleSheet("border: 1px solid black; font-size: 22px; padding: 10px;");
        pathSetLabel->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
        pathSetLabel->setMaximumWidth(220);
        INFOLayout->addWidget(pathSetLabel);

        statusLayout->addLayout(INFOLayout);

        // GIF显示区域
        gifLabel = new QLabel(this);
        gifLabel->setFixedSize(260, 200);
        gifLabel->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
        statusLayout->addWidget(gifLabel);

        mainLayout->addLayout(statusLayout);

        // 控制按钮布局
        QVBoxLayout* buttonLayout = new QVBoxLayout();
        
        QHBoxLayout* controlButtonLayout = new QHBoxLayout();
        forceButton = new QPushButton("力控", this);
        visualButton = new QPushButton("视觉", this);
        homeButton = new QPushButton("HOME", this);
        processButton = new QPushButton("碰撞", this);
        resetButton = new QPushButton("reset", this);

        forceButton->setStyleSheet("font-size: 20px;");
        forceButton->setFixedSize(100, 50);
        visualButton->setStyleSheet("font-size: 20px;");
        visualButton->setFixedSize(100, 50);
        homeButton->setStyleSheet("font-size: 20px;");
        homeButton->setFixedSize(100, 50);
        processButton->setStyleSheet("font-size: 20px;");
        processButton->setFixedSize(100, 50);
        resetButton->setStyleSheet("font-size: 20px;");
        resetButton->setFixedSize(100, 50);

        controlButtonLayout->addWidget(forceButton);
        controlButtonLayout->addWidget(visualButton);
        controlButtonLayout->addWidget(homeButton);
        controlButtonLayout->addWidget(processButton);
        controlButtonLayout->addWidget(resetButton);
        buttonLayout->addLayout(controlButtonLayout);

        // 取食路径选择按钮
        QHBoxLayout* pathButtonLayout = new QHBoxLayout();
        pathButton1 = new QPushButton("取食路径一", this);
        pathButton2 = new QPushButton("取食路径二", this);
        pathButton3 = new QPushButton("取食路径三", this);

        pathButton1->setStyleSheet("font-size: 20px;");
        pathButton1->setFixedSize(150, 50);
        pathButton2->setStyleSheet("font-size: 20px;");
        pathButton2->setFixedSize(150, 50);
        pathButton3->setStyleSheet("font-size: 20px;");
        pathButton3->setFixedSize(150, 50);

        pathButtonLayout->addWidget(pathButton1);
        pathButtonLayout->addWidget(pathButton2);
        pathButtonLayout->addWidget(pathButton3);
        buttonLayout->addLayout(pathButtonLayout);

        mainLayout->addLayout(buttonLayout);

        // 信号和槽连接
        connect(forceButton, &QPushButton::clicked, this, &ControlPanel::executeForceControl);
        connect(visualButton, &QPushButton::clicked, this, &ControlPanel::executeVisualControl);
        connect(homeButton, &QPushButton::clicked, this, &ControlPanel::GoMiddle);
        connect(processButton, &QPushButton::clicked, this, &ControlPanel::executeAllProcess);
        connect(resetButton, &QPushButton::clicked, this, &ControlPanel::restartProgram);
        connect(pathButton1, &QPushButton::clicked, this, &ControlPanel::executeTrajectory1);
        connect(pathButton2, &QPushButton::clicked, this, &ControlPanel::executeTrajectory2);
        connect(pathButton3, &QPushButton::clicked, this, &ControlPanel::executeTrajectory3);

        // 添加ROS 2订阅器 - 时间信息
        subscriber_ = node_->create_subscription<my_custom_msgs::msg::TimeInfo>(
            "time_info", 10, std::bind(&ControlPanel::timeCallback, this, std::placeholders::_1));
        RCLCPP_INFO(node_->get_logger(), "Subscribed to /time_info");

        // 添加ROS 2订阅器 - 力数据
        force_subscriber_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/force_data", 10, std::bind(&ControlPanel::forceCallback, this, std::placeholders::_1));
        RCLCPP_INFO(node_->get_logger(), "Force subscriber started");

        // 添加ROS 2订阅器 - result2_topic
        result_subscriber_ = node_->create_subscription<std_msgs::msg::Int32>(
            "/result2_topic", 10, std::bind(&ControlPanel::resultCallback, this, std::placeholders::_1));
        RCLCPP_INFO(node_->get_logger(), "Subscribed to /result2_topic");

        // 设置ROS 2事件处理定时器
        rosTimer = new QTimer(this);
        connect(rosTimer, &QTimer::timeout, [this]() {
            rclcpp::spin_some(node_);  // 定期处理ROS 2事件
        });
        rosTimer->start(100);  // 每100ms处理一次ROS 2事件

        // 设置ROS 2发布器
        publisher_ = node_->create_publisher<std_msgs::msg::String>("control_panel", 10);

        // 初始化显示空闲状态的图片
        showImage("/home/jetson/图片/home.jpeg");
    }

private slots:
void executeForceControl() {
    if (!isForceControlRunning) {
        statusLabel->setText("当前状态: 力控");
        publishMessage("执行力控操作");
        QString program = "ros2";
        QStringList arguments;
        arguments << "run" << "moveit2_tutorials" << "force_control2";
            // 确保在启动前 process 对象是有效的
            if (!process) {
        process = new QProcess(this);
            }
            // 关联 finished 信号，以便在进程意外结束时重置状态
            connect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
                    this, [this](int exitCode, QProcess::ExitStatus exitStatus){
                qDebug() << "Process finished. Exit code:" << exitCode << "Exit status:" << exitStatus;
                // 只有在用户没有主动停止时才重置状态
                if (isForceControlRunning) {
                     QMetaObject::invokeMethod(this, [this](){ // 确保在主线程更新UI
                        statusLabel->setText("当前状态: 空闲 (进程意外终止)");
                        publishMessage("力控进程意外终止");
                        isForceControlRunning = false;
                        forceButton->setText("力控");
                     }, Qt::QueuedConnection);
                }
            });
        process->start(program, arguments);
            if (!process->waitForStarted(3000)) { // 等待进程启动
                 qDebug() << "Failed to start process:" << process->errorString();
                 statusLabel->setText("当前状态: 启动失败");
                 publishMessage("启动力控进程失败");
                 isForceControlRunning = false; // 确保状态正确
                 forceButton->setText("力控"); // 恢复按钮文本
                 // 清理连接，避免重复连接
                 disconnect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), this, nullptr);
                 return;
            }
        isForceControlRunning = true;
        forceButton->setText("停止力控");
    } else {
            if (process && process->state() == QProcess::Running) {
                qint64 pid = process->processId();
                if (pid > 0) {
                    qDebug() << "Attempting to kill process group with PGID:" << pid;
                    // 发送 SIGKILL 到整个进程组
                    // 注意：processId() 返回的是 leader 的 PID，通常也是 PGID
                    // 如果 ros2 run 改变了进程组，这可能不起作用
                    int result = killpg(pid, SIGKILL);
                    if (result == 0) {
                         qDebug() << "killpg signal sent successfully.";
                    } else {
                         qDebug() << "killpg failed:" << strerror(errno);
                         // 如果 killpg 失败，尝试 kill 单个进程作为后备
                         qDebug() << "Falling back to killing individual process PID:" << pid;
                         process->kill();
                    }
                } else {
                     qDebug() << "Process ID not valid, attempting generic kill.";
                     process->kill(); // 尝试标准 kill
                }
                if (!process->waitForFinished(2000)) { // 增加等待时间
                    qDebug() << "Process did not finish after kill signal.";
                    // 可以考虑强制终止，但 killpg(SIGKILL) 应该是强制的
                } else {
                    qDebug() << "Process finished after kill signal.";
                }
            } else {
                 qDebug() << "Process is not running or process object is invalid.";
            }

            // 确保在主线程更新UI
            QMetaObject::invokeMethod(this, [this](){
            statusLabel->setText("当前状态: 空闲");
            publishMessage("停止力控操作");
            isForceControlRunning = false;
                forceButton->setText("力控");
                // 清理连接
                if(process) {
                    disconnect(process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished), this, nullptr);
                }
            }, Qt::QueuedConnection);
    }
}

    void executeVisualControl() {
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径: 一");
        publishMessage("执行路径一");
        showGif("/home/jetson/图片/轨迹1.gif");
        QString program = "ros2";
        QStringList arguments;

        arguments << "run" << "path_plan" << "path1tomiddle";
        process->start(program, arguments);
    }

    void GoMiddle() {
        publishMessage("回到middle位置");
        returnToMiddle(move_group, node_->get_logger());
        QMetaObject::invokeMethod(this, [&]() {
        statusLabel->setText("当前状态: 空闲");
        pathLabel->setText("取食路径: 无");
        showImage("/home/jetson/图片/home.jpeg");
    });
        // QMessageBox::information(this, "Info", "回到middle位置");
    }

    void executeAllProcess() {
        QString program = "ros2";
        QStringList arguments;
        arguments << "run" << "path_plan" << "path1_fc";
        process->start(program, arguments);
    }

    void restartProgram() {
        QApplication::quit();
        QProcess::startDetached(qApp->arguments()[0], qApp->arguments());
    }

    void executeTrajectory1() {
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径: 一");
        publishMessage("执行路径一");
        showGif("/home/jetson/图片/轨迹1.gif");
        executeCartesianPath1(move_group, node_->get_logger());
        // QString program = "ros2";
        // QStringList arguments;
        // arguments << "run" << "path_plan" << "force2middlepath1";
        // process->start(program, arguments);

    }

    void executeTrajectory2() {
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径: 二");
        publishMessage("执行路径二");
        showGif("/home/jetson/图片/轨迹2.gif");
        executeCartesianPath2(move_group, node_->get_logger());
        // QString program = "ros2";
        // QStringList arguments;
        // arguments << "run" << "path_plan" << "force2middlepath2";
        // process->start(program, arguments);
    }

    void executeTrajectory3() {
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径: 三");
        publishMessage("执行路径三");
        showGif("/home/jetson/图片/轨迹3.gif");
        // executeCartesianPath3(move_group, node_->get_logger());
        QString program = "ros2";
        QStringList arguments;
        arguments << "run" << "path_plan" << "force2middlepath3";
        process->start(program, arguments);
    }

    void showGif(const QString& filePath) {
        QMovie* movie = new QMovie(filePath);
        gifLabel->setMovie(movie);
        QSize labelSize = gifLabel->size();
        movie->setScaledSize(labelSize);
        movie->start();
    }

    void showImage(const QString& filePath) {
        QPixmap pixmap(filePath);
        QPixmap scaledPixmap = pixmap.scaled(gifLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation);
        gifLabel->setPixmap(scaledPixmap);
        gifLabel->setAlignment(Qt::AlignCenter);
    }

    void publishMessage(const QString& message) {
        auto msg = std_msgs::msg::String();
        msg.data = message.toStdString();
        RCLCPP_INFO(node_->get_logger(), "Publishing: '%s'", msg.data.c_str());
        publisher_->publish(msg);
    }

    void timeCallback(const my_custom_msgs::msg::TimeInfo::SharedPtr msg) {
        QMetaObject::invokeMethod(this, [this, msg]() {
            timeLabel->setText(
                QString("当前时间: %1\n运行时间: %2")
                    .arg(QString::fromStdString(msg->current_time))
                    .arg(QString::fromStdString(msg->elapsed_time))
            );
        });
    }

    // 新增力数据的回调函数
    void forceCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
        // RCLCPP_INFO(node_->get_logger(), "Received force data: X=%.2f, Y=%.2f, Z=%.2f",msg->twist.linear.x,msg->twist.linear.y,msg->twist.linear.z);
        if (std::abs(msg->twist.linear.z) >= 5.0) {
            force_sign = false;
        } else {
            force_sign = true;
        }
    }

    // 新增 result2_topic 的回调函数
    void resultCallback(const std_msgs::msg::Int32::SharedPtr msg) {
        // RCLCPP_INFO(node_->get_logger(), "Received result2_topic: %d", msg->data);
        last_result_value_.store(msg->data); // 存储接收到的值
    }

private:
    rclcpp::Node::SharedPtr node_;
    moveit::planning_interface::MoveGroupInterface move_group;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::Subscription<my_custom_msgs::msg::TimeInfo>::SharedPtr subscriber_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_subscriber_;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr result_subscriber_; // 新增 result 订阅器
    QLabel* timeLabel;
    QLabel* statusLabel;
    QLabel* pathLabel;
    QLabel* pathSetLabel;
    QPushButton* forceButton;
    QPushButton* visualButton;
    QPushButton* homeButton;
    QPushButton* processButton;
    QPushButton* resetButton;
    QPushButton* pathButton1;
    QPushButton* pathButton2;
    QPushButton* pathButton3;
    QLabel* gifLabel;
    QTimer* rosTimer;
    QProcess* process;
    bool isForceControlRunning = false;
    std::atomic<int> last_result_value_; // 新增原子变量存储结果
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("qt_ros2_control_panel");
    QApplication app(argc, argv);
    ControlPanel panel(node);
    panel.show();
    return app.exec();
}

#include "third_party.moc"